#!/usr/bin/env python3  # 使用 python3 明确指定解释器

import rospy
import actionlib
import sys
import math
import numpy as np
import time
import tf
import threading
import logging  # 添加 logging 模块
import random
from PyQt5.QtWidgets import QApplication, QMainWindow, QPushButton, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QTextEdit, QSlider, QGroupBox, QGridLayout, QSplitter, QFrame, QSizePolicy
from PyQt5.QtCore import Qt, QTimer, pyqtSignal, QThread, QRect
from PyQt5.QtGui import QFont, QIcon, QTextCursor, QPalette, QColor, QBrush, QLinearGradient, QPainter
from geometry_msgs.msg import Twist, PoseStamped, Point
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import OccupancyGrid, Path
from std_srvs.srv import Empty
from rosgraph_msgs.msg import Log  # 用于订阅ROS日志消息。

# 动画按钮类
class AnimatedButton(QPushButton):
    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self._animation = 0
        self._base_color = QColor("#4894E3")
        self._hover_color = QColor("#6CA7E3")
        self._click_color = QColor("#3A78C6")

        self.setStyleSheet("""
            QPushButton {
                border: none;
                border-radius: 12px;
                padding: 8px 15px;  /* 减小padding确保文本可见 */
                font-size: 14px;
                color: white;
                min-height: 30px;  /* 设置最小高度 */
            }
        """)

    def paintEvent(self, event):
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)

        if self.underMouse():
            color = self._hover_color
        else:
            color = self._base_color

        if self.isDown():
            color = self._click_color

        gradient = QLinearGradient(0, 0, self.width(), self.height())
        gradient.setColorAt(0, color.lighter(120))
        gradient.setColorAt(1, color.darker(120))
        painter.setBrush(gradient)
        painter.drawRoundedRect(self.rect(), 12, 12)

        super().paintEvent(event)


# 速度滑块类
class VelocitySlider(QSlider):
    def __init__(self, orientation, *args, **kwargs):
        super().__init__(orientation, *args, **kwargs)
        self.setStyleSheet("""
            QSlider::groove:horizontal {
                height: 4px;
                background: #2c2f40;
                border-radius: 2px;
            }
            QSlider::handle:horizontal {
                width: 20px;
                height: 20px;
                margin: -8px 0;
                border-radius: 10px;
                background: qlineargradient(x1:0, y1:0, x2:1, y2:1,
                    stop:0 #4894E3, stop:1 #68C190);
            }
            QSlider::sub-page:horizontal {
                background: #4894E344;
            }
        """)

# 自定义日志处理器，将 ROS 日志重定向到 Qt 界面
class QTextEditLogger(logging.Handler):
    def __init__(self, text_edit):
        super().__init__()
        self.text_edit = text_edit
        # 设置更简洁的日志格式
        formatter = logging.Formatter('[%(levelname)s] %(message)s')
        self.setFormatter(formatter)
        
    def emit(self, record):
        msg = self.format(record)
        # 根据日志级别设置不同颜色
        color = "white"
        if record.levelname == "ERROR" or record.levelname == "CRITICAL":
            color = "#FF6B6B"  # 红色
        elif record.levelname == "WARNING":
            color = "#FFD700"  # 黄色
        elif record.levelname == "INFO":
            color = "#68C190"  # 绿色
        
        # 使用更紧凑的方式添加日志
        cursor = self.text_edit.textCursor()
        cursor.movePosition(QTextCursor.End)
        if self.text_edit.toPlainText():
            cursor.insertHtml(f"<br><span style='color:{color};'>{msg}</span>")
        else:
            cursor.insertHtml(f"<span style='color:{color};'>{msg}</span>")
        
        self.text_edit.moveCursor(QTextCursor.End)

# 添加一个探索历史记录类，用于记录机器人已访问的区域
class ExplorationHistory:
    def __init__(self, cell_size=0.5):
        self.visited_goals = []  # 历史目标点
        self.cell_size = cell_size  # 栅格大小
        self.visited_cells = set()  # 已访问的栅格
        
    def add_position(self, x, y):
        """添加一个已访问的位置"""
        cell_x = int(x / self.cell_size)
        cell_y = int(y / self.cell_size)
        self.visited_cells.add((cell_x, cell_y))
        
    def is_visited(self, x, y, threshold=2):
        """检查一个位置是否在已访问的区域附近"""
        cell_x = int(x / self.cell_size)
        cell_y = int(y / self.cell_size)
        
        # 检查周围的单元格
        for dx in range(-threshold, threshold+1):
            for dy in range(-threshold, threshold+1):
                if dx == 0 and dy == 0:
                    continue
                if (cell_x + dx, cell_y + dy) in self.visited_cells:
                    return True
        return False
    
    def add_goal(self, x, y):
        """添加一个探索目标点"""
        self.visited_goals.append((x, y))
        self.add_position(x, y)
        
        # 只保留最近的50个目标点
        if len(self.visited_goals) > 50:
            self.visited_goals.pop(0)
            
    def get_recent_goals(self, count=5):
        """获取最近的几个目标点"""
        return self.visited_goals[-count:] if len(self.visited_goals) >= count else self.visited_goals

# 主控制类
class TeleopWithExplorationGUI(QMainWindow):
    # 自定义信号，用于线程间通信
    update_log_signal = pyqtSignal(str)
    update_status_signal = pyqtSignal(str)
    update_raw_log_signal = pyqtSignal(str)  # 用于原始日志消息
    
    def __init__(self):
        super().__init__()
        
        # 初始化 ROS 节点
        rospy.init_node('teleop_with_exploration_gui', anonymous=True)
        
        # 创建 Twist 消息发布者
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        
        # 订阅 cmd_vel 话题以获取实际速度值
        rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback)
        
        # 导航客户端
        self.move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        
        # 地图订阅
        self.map_data = None
        self.map_resolution = 0.05  # 默认分辨率
        self.map_origin_x = 0
        self.map_origin_y = 0
        self.map_width = 0
        self.map_height = 0
        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        
        # TF 监听器
        self.tf_listener = tf.TransformListener()
        
        # 设置操作模式
        self.mode = "manual"  # "manual" 或 "auto"
        self.auto_nav_running = False
        self.nav_thread = None
        
        # 初始化速度值
        self.linear_speed = 0.2  # 线速度 (m/s)
        self.angular_speed = 1.0  # 角速度 (rad/s)
        self.twist = Twist()
        
        # 按键状态
        self.forward = False
        self.backward = False
        self.left = False
        self.right = False
        
        # 锁定模式状态
        self.lock_mode = False  # 是否启用锁定模式
        self.direction_locked = False  # 是否已锁定方向
        
        # 初始化探索历史记录
        self.exploration_history = ExplorationHistory()
        
        # 添加探索参数
        self.frontier_min_size = 5  # 前沿点最小聚类大小
        self.frontier_distance_weight = 0.3  # 距离权重
        self.frontier_size_weight = 0.4  # 聚类大小权重
        self.frontier_orientation_weight = 0.3  # 朝向权重
        self.exploration_timeout = 60.0  # 导航超时时间（秒）
        
        # 创建一个线程锁，用于保护UI更新操作
        self.ui_lock = threading.Lock()
        
        # 注册初始化方法中的路径发布器
        self.path_pub = rospy.Publisher('/exploration_path', Path, queue_size=10)
        
        # 设置 UI
        self.init_ui()
        
        # 连接信号槽
        self.update_log_signal.connect(self.append_log)
        self.update_status_signal.connect(self.update_status)
        
        # 设置定时器，定期更新 UI
        self.timer = QTimer(self)
        self.timer.timeout.connect(self.update_ui)
        self.timer.start(100)  # 100ms 更新一次
        
        # 配置日志处理器
        self.logger = logging.getLogger('ROS')
        self.logger.setLevel(logging.INFO)
        self.log_handler = QTextEditLogger(self.log_text)
        self.logger.addHandler(self.log_handler)
        
        # 订阅ROS日志消息
        rospy.Subscriber("/rosout", Log, self.rosout_callback)
        rospy.Subscriber("/rosout_agg", Log, self.rosout_callback)
        
        # 连接原始日志信号
        self.update_raw_log_signal.connect(self.append_raw_log)
        
        # 等待导航服务启动
        rospy.loginfo("等待 move_base 服务器...")
        try:
            self.move_base_client.wait_for_server(rospy.Duration(5.0))
            rospy.loginfo("move_base 服务器已连接!")
        except:
            rospy.logwarn("无法连接到 move_base 服务器，自动导航功能可能不可用")

        # 初始化探索参数
        self.exploration_params = {
            # 打转检测参数
            'recent_goals_num': 5,
            'spin_threshold': 1.0,
            'near_goal_threshold': 1.8,
            'spin_count_threshold': 3,
            
            # 探索距离参数
            'normal_exploration_dist': 2.5,
            'global_exploration_dist': 3.5,
            
            # 其他参数
            'cluster_weight': 5.0,
            'obstacle_weight': 1.5,
        }
        
        # 添加一个按钮来打开参数窗口
        self.params_button = AnimatedButton("探索参数")
        self.params_button.clicked.connect(self.show_exploration_params)
        
        # 将按钮添加到控制按钮区域
        # 找一个合适的区域添加这个按钮，比如控制面板
        # 通常这个区域会有一个layout可以使用
        # 尝试在界面中找到合适的layout添加
        # 例如: control_layout、button_layout等

        # 初始化并自动打开参数窗口
        self.show_exploration_params()

    def cmd_vel_callback(self, msg):
        """处理cmd_vel话题的回调，更新UI上的速度显示"""
        # 更新UI上的实时速度显示，显示精确到小数点后3位的速度值
        self.current_linear_speed_label.setText(f"{msg.linear.x:.3f} m/s")
        self.current_angular_speed_label.setText(f"{msg.angular.z:.3f} rad/s")

    def rosout_callback(self, msg):
        """处理ROS日志消息"""
        try:
            # 获取日志级别和消息内容
            level_name = self.get_level_name(msg.level)
            log_message = f"[{level_name}] [{msg.header.stamp.to_sec()}, {msg.header.seq}]: {msg.msg}"
            
            # 根据日志级别设置颜色
            color = "#2196F3"  # 默认蓝色
            if msg.level == Log.ERROR or msg.level == Log.FATAL:
                color = "#FF5252"  # 红色
            elif msg.level == Log.WARN:
                color = "#FFC107"  # 黄色
            
            # 将格式化的日志消息发送到UI
            self.update_raw_log_signal.emit(f"<span style='color:{color};'>{log_message}</span>")
        except Exception as e:
            rospy.logerr(f"处理ROS日志消息时出错: {e}")
    
    def get_level_name(self, level):
        """将ROS日志级别转换为字符串"""
        if level == Log.DEBUG:
            return "DEBUG"
        elif level == Log.INFO:
            return "INFO"
        elif level == Log.WARN:
            return "WARN"
        elif level == Log.ERROR:
            return "ERROR"
        elif level == Log.FATAL:
            return "FATAL"
        else:
            return "UNKNOWN"
    
    def append_raw_log(self, msg):
        """添加原始日志到文本框"""
        self.log_text.append(msg)
        self.log_text.moveCursor(QTextCursor.End)
    
    def init_ui(self):
        """初始化用户界面"""
        self.setWindowTitle('TurtleBot3 暗夜控制台')
        # 修改初始窗口大小为更合理的尺寸
        self.setGeometry(100, 100, 800, 500)
        # 设置窗口标志，允许用户调整窗口大小
        self.setWindowFlags(self.windowFlags() | Qt.WindowMaximizeButtonHint | Qt.WindowMinimizeButtonHint)
        # 设置调整大小策略
        self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
        # 设置最小窗口大小，防止窗口被调整得太小而影响UI布局
        self.setMinimumSize(500, 350)
        self.setStyleSheet("""
            QMainWindow {
                background: qlineargradient(x1:0, y1:0, x2:1, y2:1, stop:0 #0f0f17, stop:1 #1a1b36);
                color: #ffffff;
                font-family: 'Segoe UI';
            }
            QGroupBox {
                background-color: transparent;
                border: none;
            }
            QLabel {
                color: rgba(255,255,255,0.8);
                font-size: 14px;
            }
        """)

        main_splitter = QSplitter(Qt.Horizontal)

        # 左侧面板
        left_panel = QWidget()
        left_layout = QVBoxLayout(left_panel)
        left_layout.setContentsMargins(15, 15, 15, 15)  # 减小左侧面板的边距
        left_layout.setSpacing(8)  # 减小各组件之间的间距

        # 控制模式组
        mode_group = QGroupBox()
        mode_layout = QHBoxLayout(mode_group)
        mode_layout.setContentsMargins(5, 5, 5, 5)  # 减小内边距
        self.manual_btn = AnimatedButton("手动模式")
        self.auto_btn = AnimatedButton("自动巡航")
        self.manual_btn._base_color = QColor("#68C190")
        self.auto_btn._base_color = QColor("#4894E3")
        self.manual_btn.clicked.connect(self.set_manual_mode)
        self.auto_btn.clicked.connect(self.set_auto_mode)
        self.manual_btn.setMinimumHeight(35)  # 确保按钮有足够的高度
        self.auto_btn.setMinimumHeight(35)
        mode_layout.addWidget(self.manual_btn)
        mode_layout.addWidget(self.auto_btn)
        
        # 键盘控制提示
        key_info_group = QGroupBox()
        key_info_layout = QVBoxLayout(key_info_group)
        key_info_label = QLabel(
            "<b>键盘控制:</b><br>"
            "W/↑:前进 S/↓:后退 A/←:左转 D/→:右转<br>"
            "L:切换锁定模式 空格:停止 ESC:退出"
        )
        key_info_label.setStyleSheet("color: #68C190; font-size: 12px;")
        key_info_layout.addWidget(key_info_label)
        
        # 初始化日志文本框
        self.log_text = QTextEdit()
        self.log_text.setReadOnly(True)
        self.log_text.setStyleSheet("""
            QTextEdit {
                background: #252838;
                border: none;
                border-radius: 12px;
                padding: 5px;
                color: white;
                font-size: 11px;
                line-height: 1.2;
            }
        """)
        # 设置更大的默认高度
        self.log_text.setMinimumHeight(250)
        
        # 日志面板 - 修改此部分以增大日志区域
        # 创建一个内容部件来保存日志内容
        log_content = QWidget()
        log_section = self.create_section("", log_content)
        log_section_layout = QVBoxLayout(log_content)
        log_section_layout.setContentsMargins(5, 5, 5, 5)  # 减小内边距
        log_section_layout.setSpacing(2)  # 减小间距

        log_title = QLabel("系统日志")
        log_title.setStyleSheet("font-size: 14px; color: #4894E3;")
        log_section_layout.addWidget(log_title)
        log_section_layout.addWidget(self.log_text)

        # 添加到左侧面板，减小控制模式和键盘控制组的垂直空间
        mode_section = self.create_section("控制模式", mode_group)
        mode_section.setMinimumHeight(100)  # 设置最小高度，确保能显示内容
        mode_section.setMaximumHeight(120)  # 调整最大高度，不要太小
        
        key_section = self.create_section("键盘控制", key_info_group)
        key_section.setMaximumHeight(150)  # 进一步减小键盘控制区域的高度
        
        left_layout.addWidget(mode_section)
        left_layout.addWidget(key_section)
        left_layout.addWidget(log_section, 3)  # 增大日志区域的伸缩因子
        
        # 设置键盘焦点策略，使窗口能够接收键盘事件
        self.setFocusPolicy(Qt.StrongFocus)

        # 速度控制组
        speed_group = QGroupBox()
        speed_layout = QVBoxLayout(speed_group)
        speed_layout.setSpacing(15)

        linear_layout = QHBoxLayout()
        self.linear_slider = VelocitySlider(Qt.Horizontal)
        self.linear_slider.setRange(0, 100)
        self.linear_slider.setValue(int(self.linear_speed * 100))
        self.linear_slider.valueChanged.connect(self.update_linear_speed)
        self.linear_value_label = QLabel(f"{self.linear_speed:.2f} m/s")
        self.linear_value_label.setAlignment(Qt.AlignRight)
        self.linear_value_label.setStyleSheet("color: #4894E3; font-weight: bold;")
        linear_layout.addWidget(QLabel("线速度"))
        linear_layout.addWidget(self.linear_slider)
        linear_layout.addWidget(self.linear_value_label)

        angular_layout = QHBoxLayout()
        self.angular_slider = VelocitySlider(Qt.Horizontal)
        self.angular_slider.setRange(0, 200)
        self.angular_slider.setValue(int(self.angular_speed * 100))
        self.angular_slider.valueChanged.connect(self.update_angular_speed)
        self.angular_value_label = QLabel(f"{self.angular_speed:.2f} rad/s")
        self.angular_value_label.setAlignment(Qt.AlignRight)
        self.angular_value_label.setStyleSheet("color: #68C190; font-weight: bold;")
        angular_layout.addWidget(QLabel("角速度"))
        angular_layout.addWidget(self.angular_slider)
        angular_layout.addWidget(self.angular_value_label)

        speed_layout.addLayout(linear_layout)
        speed_layout.addLayout(angular_layout)

        # 状态显示组
        status_group = QGroupBox()
        status_layout = QVBoxLayout(status_group)
        status_layout.setContentsMargins(10, 10, 10, 10)

        status_frame = QFrame()
        status_frame.setStyleSheet("background: #252838; border-radius: 12px;")
        frame_layout = QVBoxLayout(status_frame)

        mode_status = QHBoxLayout()
        mode_status.addWidget(QLabel("模式:"))
        self.status_label = QLabel("手动")
        self.status_label.setStyleSheet("color: #68C190; font-weight: bold;")
        mode_status.addWidget(self.status_label)
        mode_status.addStretch()

        position_status = QHBoxLayout()
        position_status.addWidget(QLabel("位置:"))
        self.position_label = QLabel("(0.00, 0.00)")
        self.position_label.setStyleSheet("color: #4894E3; font-weight: bold;")
        position_status.addWidget(self.position_label)
        position_status.addStretch()
        
        lock_status = QHBoxLayout()
        lock_status.addWidget(QLabel("锁定模式:"))
        self.lock_mode_label = QLabel("关闭")
        self.lock_mode_label.setStyleSheet("color: #FF6B6B; font-weight: bold;")
        lock_status.addWidget(self.lock_mode_label)
        lock_status.addStretch()
        
        # 添加实时线速度显示
        linear_speed_status = QHBoxLayout()
        linear_speed_status.addWidget(QLabel("当前线速度:"))
        self.current_linear_speed_label = QLabel("0.00 m/s")
        self.current_linear_speed_label.setStyleSheet("color: #4894E3; font-weight: bold;")
        linear_speed_status.addWidget(self.current_linear_speed_label)
        linear_speed_status.addStretch()
        
        # 添加实时角速度显示
        angular_speed_status = QHBoxLayout()
        angular_speed_status.addWidget(QLabel("当前角速度:"))
        self.current_angular_speed_label = QLabel("0.00 rad/s")
        self.current_angular_speed_label.setStyleSheet("color: #68C190; font-weight: bold;")
        angular_speed_status.addWidget(self.current_angular_speed_label)
        angular_speed_status.addStretch()

        frame_layout.addLayout(mode_status)
        frame_layout.addLayout(position_status)
        frame_layout.addLayout(lock_status)
        frame_layout.addLayout(linear_speed_status)
        frame_layout.addLayout(angular_speed_status)
        status_layout.addWidget(status_frame)

        # 右侧面板
        right_panel = QWidget()
        right_layout = QVBoxLayout(right_panel)
        right_layout.setContentsMargins(20, 20, 20, 20)
        right_layout.setSpacing(15)
        
        # 添加速度设置到右侧面板
        right_layout.addWidget(self.create_section("速度设置", speed_group))
        
        # 添加状态信息到右侧面板
        right_layout.addWidget(self.create_section("状态信息", status_group))
        right_layout.addStretch()

        # 分割器设置
        main_splitter.addWidget(left_panel)
        main_splitter.addWidget(right_panel)
        main_splitter.setSizes([500, 300])  # 调整左右面板的比例，给左侧更多空间
        self.setCentralWidget(main_splitter)

        # 初始化日志
        rospy.loginfo("系统初始化完成")
        rospy.loginfo("等待操作指令...")
        
        # 设置动画
        self.setup_animations()
    
    def create_section(self, title, widget):
        section = QWidget()
        section.setStyleSheet("""
            QWidget {
                background: #1e2234;
                border-radius: 12px;
                padding: 8px;  /* 减小内边距 */
            }
        """)
        layout = QVBoxLayout()
        layout.setContentsMargins(5, 5, 5, 5)  # 减小边距
        layout.setSpacing(8)  # 适当增加间距
        
        if title:  # 只有当标题不为空时才添加标题
            title_label = QLabel(title)
            title_label.setStyleSheet("font-size: 14px; color: #ffffff; font-weight: bold;")
            layout.addWidget(title_label)
        
        layout.addWidget(widget)
        section.setLayout(layout)
        return section

    def create_joystick_button(self, text, color):
        btn = QPushButton(text)
        btn.setStyleSheet(f"""
            QPushButton {{
                background: {color};
                border-radius: 25px;
                min-width: 50px;
                min-height: 50px;
                font-size: 20px;
                color: white;
                margin: 5px;
            }}
            QPushButton:hover {{
                background: {color}CC;
            }}
            QPushButton:pressed {{
                background: {color}99;
            }}
        """)
        return btn

    def setup_animations(self):
        # 滑块数值更新定时器
        self.slider_timer = QTimer()
        self.slider_timer.timeout.connect(self.update_slider_values)
        self.slider_timer.start(100)

    def update_slider_values(self):
        self.linear_value_label.setText(f"{self.linear_slider.value() / 100:.2f} m/s")
        self.angular_value_label.setText(f"{self.angular_slider.value() / 100:.2f} rad/s")
        
    def update_ui(self):
        """定期更新 UI 信息"""
        try:
            x, y = self.get_robot_position()
            self.position_label.setText(f"({x:.2f}, {y:.2f})")
        except:
            pass
    
    def handle_direction_key(self, direction, pressed):
        """处理方向键按下和释放事件"""
        # 如果是锁定模式且按键被按下
        if self.lock_mode and pressed:
            # 如果当前没有锁定方向，则锁定该方向
            if not self.direction_locked:
                if direction == "up":
                    self.forward = True
                    self.backward = False
                    self.left = False
                    self.right = False
                elif direction == "down":
                    self.forward = False
                    self.backward = True
                    self.left = False
                    self.right = False
                elif direction == "left":
                    self.forward = False
                    self.backward = False
                    self.left = True
                    self.right = False
                elif direction == "right":
                    self.forward = False
                    self.backward = False
                    self.left = False
                    self.right = True
                self.direction_locked = True
                rospy.loginfo(f"方向已锁定: {direction}")
            # 如果已经锁定了方向，再次按下同一方向键则解除锁定
            else:
                if (direction == "up" and self.forward) or \
                   (direction == "down" and self.backward) or \
                   (direction == "left" and self.left) or \
                   (direction == "right" and self.right):
                    self.forward = False
                    self.backward = False
                    self.left = False
                    self.right = False
                    self.direction_locked = False
                    rospy.loginfo("方向锁定已解除")
                # 如果按下的是不同方向的键，则切换到新方向
                else:
                    if direction == "up":
                        self.forward = True
                        self.backward = False
                        self.left = False
                        self.right = False
                    elif direction == "down":
                        self.forward = False
                        self.backward = True
                        self.left = False
                        self.right = False
                    elif direction == "left":
                        self.forward = False
                        self.backward = False
                        self.left = True
                        self.right = False
                    elif direction == "right":
                        self.forward = False
                        self.backward = False
                        self.left = False
                        self.right = True
                    rospy.loginfo(f"方向已切换: {direction}")
        # 非锁定模式，按照原来的逻辑处理
        else:
            if direction == "up":
                self.forward = pressed
            elif direction == "down":
                self.backward = pressed
            elif direction == "left":
                self.left = pressed
            elif direction == "right":
                self.right = pressed
            
            # 如果是按键释放事件，且不在锁定模式下，则不改变速度状态
            # 这样可以保持当前速度直到用户主动停止
            if not pressed and not self.lock_mode:
                return
            
        if self.mode == "manual":
            self.update_speed()
    
    def update_linear_speed(self):
        """更新线速度"""
        self.linear_speed = self.linear_slider.value() / 100.0
        self.linear_value_label.setText(f"{self.linear_speed:.2f} m/s")
        if self.mode == "manual":
            self.update_speed()
    
    def update_angular_speed(self):
        """更新角速度"""
        self.angular_speed = self.angular_slider.value() / 100.0
        self.angular_value_label.setText(f"{self.angular_speed:.2f} rad/s")
        if self.mode == "manual":
            self.update_speed()
    
    def update_speed(self):
        """根据按键状态更新速度"""
        if self.mode != "manual":
            return
            
        self.twist.linear.x = self.linear_speed if self.forward else (-self.linear_speed if self.backward else 0.0)
        self.twist.linear.y = 0.0
        self.twist.linear.z = 0.0
        self.twist.angular.x = 0.0
        self.twist.angular.y = 0.0
        self.twist.angular.z = self.angular_speed if self.left else (-self.angular_speed if self.right else 0.0)
        
        # 更新UI上的实时速度显示，显示精确到小数点后3位的速度值
        self.current_linear_speed_label.setText(f"{self.twist.linear.x:.3f} m/s")
        self.current_angular_speed_label.setText(f"{self.twist.angular.z:.3f} rad/s")
        
        self.cmd_vel_pub.publish(self.twist)
    
    def set_manual_mode(self):
        """设置为手动控制模式"""
        # 停止当前的导航任务
        if self.auto_nav_running:
            self.auto_nav_running = False
            # 确保取消move_base的当前目标
            if self.move_base_client:
                self.move_base_client.cancel_all_goals()  # 取消所有导航目标
            # 确保机器人立即停止
            self.stop_robot()
            rospy.sleep(0.5)  # 给系统一点时间处理停止命令
        
        self.mode = "manual"
        self.append_log("已切换到手动模式")
        self.manual_btn.setEnabled(False)
        self.auto_btn.setEnabled(True)
        self.status_label.setText("手动")
        self.status_label.setStyleSheet("color: #4894E3; font-weight: bold;")
        
        # 更新UI
        QApplication.processEvents()

    def set_auto_mode(self):
        """设置为自动探索模式"""
        if self.auto_nav_running:
            # 如果已经在自动模式，则停止
            self.auto_nav_running = False
            if self.move_base_client:
                # 修改：使用单个cancel_goal而不是cancel_all_goals
                self.move_base_client.cancel_goal()
            self.stop_robot()
            self.append_log("停止自动探索")
            self.status_label.setText("手动")
            self.status_label.setStyleSheet("color: #4894E3; font-weight: bold;")
            self.mode = "manual"
            self.auto_btn.setText("自动巡航")
            return
        
        if self.move_base_client is None:
            self.append_log("<span style='color:#FF6B6B;'>✗ 无法连接到导航服务</span>")
            return
        
        # 启动自动探索
        self.auto_nav_running = True
        self.append_log("开始自动探索")
        self.status_label.setText("自动探索中")
        self.status_label.setStyleSheet("color: #68C190; font-weight: bold;")
        
        # 在新线程中启动自动探索
        threading.Thread(target=self.auto_exploration_loop).start()

    def stop_robot(self):
        """立即停止机器人"""
        # 发送多个停止命令确保机器人接收
        twist = Twist()
        for _ in range(3):  # 发送多次停止命令
            self.cmd_vel_pub.publish(twist)
            rospy.sleep(0.05)
        
        # 如果有活跃的导航目标，取消它
        if self.auto_nav_running and self.move_base_client:
            self.move_base_client.cancel_all_goals()
        
        rospy.loginfo("机器人已停止")

    def start_auto_navigation(self):
        """启动自动导航线程"""
        if not self.auto_nav_running:
            self.auto_nav_running = True
            self.nav_thread = threading.Thread(target=self.auto_exploration_loop)
            self.nav_thread.daemon = True
            self.nav_thread.start()
            rospy.loginfo("开始自动探索")

    def stop_auto_navigation(self):
        """停止自动导航"""
        if self.auto_nav_running:
            self.auto_nav_running = False
            self.move_base_client.cancel_all_goals()
            rospy.loginfo("已停止自动导航")
            if self.nav_thread:
                self.nav_thread.join(timeout=1.0)
                
    def map_callback(self, data):
        """接收地图数据"""
        self.map_data = data
        self.map_resolution = data.info.resolution
        self.map_origin_x = data.info.origin.position.x
        self.map_origin_y = data.info.origin.position.y
        self.map_width = data.info.width
        self.map_height = data.info.height
        
    def get_robot_position(self):
        """获取机器人当前位置"""
        try:
            (trans, rot) = self.tf_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
            return trans[0], trans[1]
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logwarn("无法获取机器人位置，使用 (0,0) 作为默认位置")
            return 0.0, 0.0
            
    def log_info(self, msg):
        """记录信息日志（备用手动方式）"""
        rospy.loginfo(msg)
        self.update_log_signal.emit(f"[INFO] {msg}")
        
    def log_warn(self, msg):
        """记录警告日志（备用手动方式）"""
        rospy.logwarn(msg)
        self.update_log_signal.emit(f"[WARN] {msg}")
        
    def log_error(self, msg):
        """记录错误日志（备用手动方式）"""
        rospy.logerr(msg)
        self.update_log_signal.emit(f"[ERROR] {msg}")
        
    def append_log(self, message):
        """安全地添加日志到文本框"""
        try:
            # 使用简单文本而非复杂HTML
            # 移除特殊Unicode字符，如警告符号
            simplified_message = message
            if "⚠" in message:
                simplified_message = message.replace("⚠", "!")
            if "✓" in message:
                simplified_message = message.replace("✓", "+")
            if "✗" in message:
                simplified_message = message.replace("✗", "x")
            
            # 使用现有的信号机制
            if hasattr(self, 'log_signal'):
                # 使用信号更新
                self.log_signal.emit(simplified_message)
            else:
                # 直接更新（仅在主线程中调用）
                # 支持HTML格式的日志消息
                if not simplified_message.startswith('<span'):
                    simplified_message = f"<span style='color:#89B4FA;'>{simplified_message}</span>"
                
                with self.ui_lock:
                    # 添加日志时不添加额外的换行符
                    cursor = self.log_text.textCursor()
                    cursor.movePosition(QTextCursor.End)
                    if self.log_text.toPlainText():
                        cursor.insertHtml("<br>" + simplified_message)
                    else:
                        cursor.insertHtml(simplified_message)
                    
                    # 限制日志条目数量，保留最新的150条日志
                    doc = self.log_text.document()
                    if doc.blockCount() > 150:
                        cursor = QTextCursor(doc)
                        cursor.movePosition(QTextCursor.Start)
                        cursor.movePosition(QTextCursor.Down, QTextCursor.KeepAnchor, 50)
                        cursor.removeSelectedText()
                        
                    self.log_text.moveCursor(QTextCursor.End)
        except Exception as e:
            rospy.logerr(f"日志添加失败: {e}")
        
    def update_status(self, status):
        """更新状态标签"""
        self.status_label.setText(status)
        self.status_label.setStyleSheet("color: #A6E3A1; font-size: 14px;")
        
    def find_frontier_points(self):
        """寻找前沿点，更关注未知区域探索并确保可到达性"""
        if self.map_data is None:
            rospy.logwarn("还没有接收到地图数据")
            return []
        
        map_data = self.map_data
        width = map_data.info.width
        height = map_data.info.height
        resolution = map_data.info.resolution
        
        # 地图数据处理
        data = np.array(map_data.data).reshape(height, width)
        
        # 设置阈值
        free_threshold = 10     # 确定为空闲的阈值
        unknown_value = -1      # 未知区域的值
        obstacle_threshold = 65 # 障碍物阈值
        
        # 获取机器人当前位置
        robot_x, robot_y = self.get_robot_position()
        # 转换为地图坐标
        robot_mx = int((robot_x - map_data.info.origin.position.x) / resolution)
        robot_my = int((robot_y - map_data.info.origin.position.y) / resolution)
        
        # 存储前沿点
        frontiers = []
        
        # 优化：以更大步长遍历地图提高效率
        step = 4
        for y in range(step, height - step, step):
            for x in range(step, width - step, step):
                # 确保坐标有效
                if y < 0 or y >= height or x < 0 or x >= width:
                    continue
                
                # 检查是否为空闲单元格
                if data[y, x] >= 0 and data[y, x] <= free_threshold:
                    # 检查是否有未知邻居
                    has_unknown = False
                    
                    # 检查周围8个方向
                    for dx in [-1, 0, 1]:
                        for dy in [-1, 0, 1]:
                            if dx == 0 and dy == 0:
                                continue
                            
                            # 确保邻居坐标有效
                            ny, nx = y + dy, x + dx
                            if ny < 0 or ny >= height or nx < 0 or nx >= width:
                                continue
                            
                            if data[ny, nx] == unknown_value:
                                has_unknown = True
                                break
                        
                        if has_unknown:
                            break
                    
                    # 如果有未知邻居，进一步检查是否为好的前沿点
                    if has_unknown:
                        # 检查更大范围是否有足够的自由空间（确保可达性）
                        free_space_count = 0
                        total_cells = 0
                        obstacle_nearby = False
                        
                        # 检查5x5区域
                        check_size = 5
                        for cx in range(x - check_size, x + check_size + 1):
                            for cy in range(y - check_size, y + check_size + 1):
                                if 0 <= cx < width and 0 <= cy < height:
                                    total_cells += 1
                                    
                                    # 统计自由空间
                                    if data[cy, cx] >= 0 and data[cy, cx] <= free_threshold:
                                        free_space_count += 1
                                        
                                    # 检查是否有障碍物
                                    if data[cy, cx] > obstacle_threshold:
                                        # 计算到中心点的距离
                                        dist = math.sqrt((cx - x)**2 + (cy - y)**2)
                                        if dist < 3:  # 如果障碍物太近，拒绝这个点
                                            obstacle_nearby = True
                        
                        # 计算到机器人的距离（地图坐标系）
                        map_dist = math.sqrt((x - robot_mx)**2 + (y - robot_my)**2)
                        
                        # 条件1：足够的自由空间
                        # 条件2：没有近距离障碍物
                        # 条件3：与机器人保持合理距离
                        if (free_space_count >= total_cells * 0.6 and 
                            not obstacle_nearby and
                            5 <= map_dist <= 40):  # 距离适中（地图单位）
                            
                            # 将地图坐标转换为世界坐标
                            world_x = map_data.info.origin.position.x + (x + 0.5) * resolution
                            world_y = map_data.info.origin.position.y + (y + 0.5) * resolution
                            
                            # 再次检查世界坐标系距离
                            world_dist = math.sqrt((world_x - robot_x)**2 + (world_y - robot_y)**2)
                            if 0.4 <= world_dist <= 1.5:  # 0.4-1.5米是合理探索距离
                                frontiers.append((world_x, world_y))
        
        # 如果没有找到前沿点，返回空列表
        if not frontiers:
            return []
        
        # 按距离排序
        distances = [math.sqrt((x - robot_x)**2 + (y - robot_y)**2) for x, y in frontiers]
        sorted_indices = np.argsort(distances)
        sorted_frontiers = [frontiers[i] for i in sorted_indices]
        
        # 只返回最近的几个点
        return sorted_frontiers[:3]

    def cluster_frontier_points(self, frontiers, cluster_tolerance=0.5):
        """将前沿点聚类，并计算每个聚类的大小和质量"""
        if not frontiers:
            return []
        
        clusters = []
        visited = set()
        
        for i, point in enumerate(frontiers):
            if i in visited:
                continue
            
            cluster = [point]
            visited.add(i)
            
            stack = [i]
            while stack:
                current = stack.pop()
                
                for j, other_point in enumerate(frontiers):
                    if j in visited:
                        continue
                    
                    dist = math.sqrt((frontiers[current][0] - other_point[0])**2 + 
                            (frontiers[current][1] - other_point[1])**2)
                    if dist < cluster_tolerance:
                        cluster.append(other_point)
                        visited.add(j)
                        stack.append(j)
            
            if len(cluster) >= self.frontier_min_size:
                clusters.append(cluster)
        
        # 计算每个聚类的中心和大小
        cluster_info = []
        for cluster in clusters:
            x_sum = sum(p[0] for p in cluster)
            y_sum = sum(p[1] for p in cluster)
            center_x = x_sum / len(cluster)
            center_y = y_sum / len(cluster)
            
            # 计算聚类的直径（最远两点之间的距离）
            max_dist = 0
            for i in range(len(cluster)):
                for j in range(i+1, len(cluster)):
                    dist = math.sqrt((cluster[i][0] - cluster[j][0])**2 + 
                            (cluster[i][1] - cluster[j][1])**2)
                    max_dist = max(max_dist, dist)
            
            cluster_info.append({
                "center": (center_x, center_y),
                "size": len(cluster),
                "diameter": max_dist
            })
            
        return cluster_info

    def get_robot_orientation(self):
        """获取机器人当前朝向（欧拉角的偏航角）"""
        try:
            (trans, rot) = self.tf_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
            euler = tf.transformations.euler_from_quaternion(rot)
            return euler[2]  # 偏航角（绕z轴旋转）
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logwarn("无法获取机器人朝向，使用默认值0")
            return 0.0

    def calculate_angle(self, x1, y1, x2, y2):
        """计算从点(x1,y1)到点(x2,y2)的角度"""
        return math.atan2(y2 - y1, x2 - x1)

    def auto_exploration_loop(self):
        """自主探索主循环，更智能地处理规划错误和打转行为"""
        try:
            # 确保已连接到move_base服务器
            if not self.move_base_client or not self.move_base_client.wait_for_server(rospy.Duration(5.0)):
                rospy.logerr("无法连接到move_base服务器")
                self.auto_nav_running = False
                self.status_label.setText("手动")
                self.status_label.setStyleSheet("color: #4894E3; font-weight: bold;")
                return
            
            # 更新UI状态
            rospy.loginfo("开始自动探索")
            self.append_log("<span style='color:#68C190;'>开始自动探索</span>")
            
            # 初始化变量
            consecutive_failures = 0  # 连续失败次数
            consecutive_planning_errors = 0  # 连续规划错误
            current_goal_active = False  # 标记当前是否有活跃的导航目标
            spinning_count = 0  # 跟踪连续打转次数
            
            # 清除代价地图开始探索
            try:
                clear_costmap = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
                clear_costmap()
                rospy.loginfo("已清除代价地图，开始探索")
            except:
                rospy.logwarn("无法清除代价地图，继续探索")
            
            while self.auto_nav_running and not rospy.is_shutdown():
                # 检查是否应该停止
                if not self.auto_nav_running:
                    break
                
                # 获取机器人位置
                robot_x, robot_y = self.get_robot_position()
                
                # 查找前沿点
                frontiers = self.find_frontier_points()
                
                # 如果没有找到前沿点或连续错误太多，执行恢复操作
                if not frontiers or consecutive_failures > 2 or consecutive_planning_errors > 1:
                    rospy.logwarn(f"难以找到可到达的前沿点: 失败={consecutive_failures}, 规划错误={consecutive_planning_errors}")
                    self.append_log("<span style='color:#FFD700;'>! 执行探索策略</span>")
                    
                    try:
                        # 更积极地探索
                        if self.explore_distant_area():
                            rospy.loginfo("积极探索成功")
                            consecutive_failures = 0
                            consecutive_planning_errors = 0
                        else:
                            # 探索失败才使用恢复行为
                            rospy.logwarn("积极探索失败，尝试基础恢复")
                            self.recover_from_navigation_failure()
                            self.simple_exploration_move(consecutive_failures)
                    except Exception as e:
                        rospy.logerr(f"执行探索操作时出错: {e}")
                        rospy.sleep(1.0)
                    
                    # 重置错误计数
                    if consecutive_planning_errors > 0:
                        consecutive_planning_errors -= 1
                    
                    # 短暂等待
                    rospy.sleep(1.0)
                    continue
                
                # 检查是否在打转
                if self.detect_spinning_behavior():
                    spinning_count += 1
                    rospy.logwarn(f"可能在原地打转: {spinning_count}/3")
                    
                    # 连续检测到3次打转行为，执行全局探索
                    if spinning_count >= 3:
                        rospy.logwarn("连续检测到打转行为，尝试全局探索")
                        
                        # 取消当前目标
                        if current_goal_active:
                            self.move_base_client.cancel_goal()
                            current_goal_active = False
                            rospy.sleep(1.0)
                        
                        # 执行全局探索
                        if self.force_global_exploration():
                            # 全局探索成功，重置计数器
                            spinning_count = 0
                            consecutive_failures = 0
                            consecutive_planning_errors = 0
                        else:
                            # 全局探索失败，执行常规恢复
                            self.recover_from_navigation_failure()
                            self.simple_exploration_move(consecutive_failures)
                        
                        continue
                else:
                    # 如果不是打转，减少计数
                    spinning_count = max(0, spinning_count - 1)
                
                # 尝试每个前沿点，直到找到可规划的路径
                goal_sent = False
                plan_failed = True
                
                for target_x, target_y in frontiers:
                    # 创建导航目标
                    goal = MoveBaseGoal()
                    goal.target_pose.header.frame_id = "map"
                    goal.target_pose.header.stamp = rospy.Time.now()
                    goal.target_pose.pose.position.x = target_x
                    goal.target_pose.pose.position.y = target_y
                    goal.target_pose.pose.position.z = 0.0
                    
                    # 设置目标朝向（朝向未知区域）
                    target_orientation = math.atan2(target_y - robot_y, target_x - robot_x)
                    goal.target_pose.pose.orientation.w = math.cos(target_orientation / 2)
                    goal.target_pose.pose.orientation.x = 0.0
                    goal.target_pose.pose.orientation.y = 0.0
                    goal.target_pose.pose.orientation.z = math.sin(target_orientation / 2)
                    
                    # 发送目标
                    rospy.loginfo(f"当前导航到 ({target_x:.2f}, {target_y:.2f})")
                    self.publish_path_to_goal(target_x, target_y)
                    self.move_base_client.send_goal(goal)
                    current_goal_active = True
                    goal_sent = True
                    
                    # 给规划器一点时间尝试规划
                    planning_success = self.move_base_client.wait_for_result(rospy.Duration(2.0))
                    
                    if not planning_success:
                        # 检查是否有"global plan"错误日志（这需要额外实现）
                        # 简单的方法是等待一小段时间，看是否有进展
                        rospy.sleep(1.0)
                        planning_success = self.move_base_client.wait_for_result(rospy.Duration(1.0))
                    
                    if planning_success or self.check_if_making_progress():
                        plan_failed = False
                        break
                    else:
                        # 取消当前目标，尝试下一个
                        self.move_base_client.cancel_goal()
                        current_goal_active = False
                        rospy.logwarn(f"无法规划到 ({target_x:.2f}, {target_y:.2f})，尝试下一个点")
                        rospy.sleep(0.5)  # 给系统一点时间恢复
                
                # 如果所有点都无法规划
                if plan_failed:
                    consecutive_planning_errors += 1
                    rospy.logwarn(f"所有前沿点都无法规划路径，增加规划错误计数: {consecutive_planning_errors}")
                    continue
                
                # 等待导航结果
                nav_start_time = time.time()
                max_nav_time = 30.0  # 最大导航时间（秒）
                
                while (time.time() - nav_start_time < max_nav_time and 
                       not self.move_base_client.wait_for_result(rospy.Duration(0.5))):
                    
                    # 检查是否仍在自动模式
                    if not self.auto_nav_running:
                        self.move_base_client.cancel_goal()
                        current_goal_active = False
                        break
                
                # 检查导航结果
                if current_goal_active:
                    state = self.move_base_client.get_state()
                    if state == actionlib.GoalStatus.SUCCEEDED:
                        rospy.loginfo("✓ 成功到达目标")
                        self.append_log("<span style='color:#68C190;'>✓ 导航成功</span>")
                        
                        # 重置错误计数
                        consecutive_failures = 0
                        consecutive_planning_errors = 0
                        
                        # 旋转扫描环境
                        self.scan_surrounding()
                    else:
                        # 导航失败
                        rospy.logwarn(f"导航失败: {self.get_goal_status_text(state)}")
                        self.append_log("<span style='color:#FFD700;'>⚠ 导航未完成</span>")
                        consecutive_failures += 1
                
                # 确保没有活跃目标
                if current_goal_active:
                    self.move_base_client.cancel_goal()
                    current_goal_active = False
                
                # 短暂等待
                rospy.sleep(1.0)
        
        except Exception as e:
            rospy.logerr(f"自动探索发生错误: {e}")
            import traceback
            rospy.logerr(traceback.format_exc())
        finally:
            # 清理资源
            if self.move_base_client:
                self.move_base_client.cancel_goal()
            self.stop_robot()
            self.auto_nav_running = False
            self.status_label.setText("手动")
            self.status_label.setStyleSheet("color: #4894E3; font-weight: bold;")
            rospy.loginfo("自动探索已停止")

    def recover_from_navigation_failure(self):
        """
        从导航失败中恢复的辅助函数
        
        该函数执行一系列操作来帮助机器人从导航失败中恢复：
        1. 取消当前导航目标
        2. 清除导航代价地图中的障碍物信息
        3. 执行小范围旋转以重新获取环境信息
        4. 发布停止命令
        
        这有助于解决因临时障碍物、定位误差或地图错误导致的导航问题。
        """
        rospy.loginfo("执行导航恢复程序...")
        
        # 1. 取消当前目标
        try:
            self.move_base_client.cancel_goal()
            rospy.loginfo("已取消当前导航目标")
        except:
            rospy.logwarn("取消导航目标失败")
        
        rospy.sleep(1.0)  # 等待取消操作完成
        
        # 2. 尝试清除代价地图
        try:
            clear_costmap = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
            clear_costmap()
            rospy.loginfo("已清除导航代价地图")
        except rospy.ServiceException as e:
            rospy.logwarn(f"清除代价地图失败: {e}")
        
        # 3. 执行小范围旋转以获取更好的环境感知
        rospy.loginfo("执行环境重新扫描...")
        twist = Twist()
        twist.angular.z = 0.5  # 设置角速度为0.5 rad/s
        
        # 旋转3秒
        start_time = time.time()
        while time.time() - start_time < 3.0 and not rospy.is_shutdown():
            self.cmd_vel_pub.publish(twist)
            rospy.sleep(0.1)
        
        # 停止旋转
        twist.angular.z = 0.0
        self.cmd_vel_pub.publish(twist)
        
        # 等待一小段时间让激光雷达数据更新
        rospy.sleep(1.0)
        
        rospy.loginfo("导航恢复操作完成")

    def get_goal_status_text(self, status_code):
        """
        将导航状态代码转换为可读的文本描述
        
        参数:
            status_code: actionlib状态代码
            
        返回值:
            str: 状态的文本描述
        """
        status_dict = {
            actionlib.GoalStatus.PENDING: "等待中",
            actionlib.GoalStatus.ACTIVE: "执行中",
            actionlib.GoalStatus.PREEMPTED: "被抢占",
            actionlib.GoalStatus.SUCCEEDED: "成功",
            actionlib.GoalStatus.ABORTED: "中止",
            actionlib.GoalStatus.REJECTED: "拒绝",
            actionlib.GoalStatus.PREEMPTING: "正在抢占",
            actionlib.GoalStatus.RECALLING: "正在撤回",
            actionlib.GoalStatus.RECALLED: "已撤回",
            actionlib.GoalStatus.LOST: "丢失"
        }
        return status_dict.get(status_code, f"未知状态({status_code})")

    def scan_surrounding(self):
        """在当前位置旋转一周，以扫描周围环境"""
        rospy.loginfo("旋转扫描周围环境")
        
        twist = Twist()
        twist.angular.z = 0.5  # 以适中的速度旋转
        
        duration = 2 * math.pi / 0.5  # 旋转一周所需时间
        start_time = time.time()
        
        # 旋转一周
        while time.time() - start_time < duration and self.auto_nav_running:
            self.cmd_vel_pub.publish(twist)
            rospy.sleep(0.1)
        
        # 停止旋转
        twist.angular.z = 0.0
        self.cmd_vel_pub.publish(twist)
        
        rospy.loginfo("环境扫描完成")

    def rotate_to_find_frontiers(self):
        """原地旋转以寻找新的前沿"""
        rospy.loginfo("执行原地旋转以寻找新区域")
        
        twist = Twist()
        twist.angular.z = 0.5
        
        # 速度显示会通过cmd_vel_callback自动更新
        
        start_time = time.time()
        while time.time() - start_time < 10.0 and self.auto_nav_running:
            self.cmd_vel_pub.publish(twist)
            rospy.sleep(0.1)
            
        twist.angular.z = 0.0
        self.cmd_vel_pub.publish(twist)
        
        # 速度显示会通过cmd_vel_callback自动更新
        self.current_linear_speed_label.setText("0.00 m/s")
        self.current_angular_speed_label.setText("0.50 rad/s")
        
        start_time = time.time()
        while time.time() - start_time < 10.0 and self.auto_nav_running:
            self.cmd_vel_pub.publish(twist)
            rospy.sleep(0.1)
            
        twist.angular.z = 0.0
        self.cmd_vel_pub.publish(twist)
        
        # 速度显示会通过cmd_vel_callback自动更新
        self.current_linear_speed_label.setText("0.00 m/s")
        self.current_angular_speed_label.setText("0.00 rad/s")

    def toggle_lock_mode(self):
        """切换锁定模式"""
        self.lock_mode = not self.lock_mode
        if self.lock_mode:
            self.lock_mode_label.setText("开启")
            self.lock_mode_label.setStyleSheet("color: #68C190; font-weight: bold;")
            rospy.loginfo("锁定模式已开启")
        else:
            self.lock_mode_label.setText("关闭")
            self.lock_mode_label.setStyleSheet("color: #FF6B6B; font-weight: bold;")
            self.direction_locked = False
            rospy.loginfo("锁定模式已关闭")
    
    def keyPressEvent(self, event):
        """处理键盘按下事件"""
        key = event.key()
        
        # 紧急停止 - 无论什么模式都生效
        if key == Qt.Key_Space:
            # 强制停止所有动作
            if self.auto_nav_running:
                self.auto_nav_running = False
                if self.move_base_client:
                    self.move_base_client.cancel_all_goals()
            self.stop_robot()
            # 如果在锁定模式下，解除方向锁定
            if self.lock_mode and self.direction_locked:
                self.direction_locked = False
                rospy.loginfo("方向锁定已解除")
            return
        
        # 以下控制仅在手动模式下有效
        if self.mode == "manual":
            # 方向控制
            if key == Qt.Key_W or key == Qt.Key_Up:
                self.handle_direction_key("up", True)
            elif key == Qt.Key_S or key == Qt.Key_Down:
                self.handle_direction_key("down", True)
            elif key == Qt.Key_A or key == Qt.Key_Left:
                self.handle_direction_key("left", True)
            elif key == Qt.Key_D or key == Qt.Key_Right:
                self.handle_direction_key("right", True)
            # 锁定模式切换
            elif key == Qt.Key_L:
                self.toggle_lock_mode()
        # 退出
        elif key == Qt.Key_Escape:
            # 停止所有活动
            if self.auto_nav_running:
                self.auto_nav_running = False
                if self.move_base_client:
                    self.move_base_client.cancel_all_goals()
            self.stop_robot()
            self.close()
    
    def keyReleaseEvent(self, event):
        """处理键盘释放事件"""
        if self.mode != "manual":
            return
            
        key = event.key()
        
        # 方向控制
        if key == Qt.Key_W or key == Qt.Key_Up:
            self.handle_direction_key("up", False)
        elif key == Qt.Key_S or key == Qt.Key_Down:
            self.handle_direction_key("down", False)
        elif key == Qt.Key_A or key == Qt.Key_Left:
            self.handle_direction_key("left", False)
        elif key == Qt.Key_D or key == Qt.Key_Right:
            self.handle_direction_key("right", False)
    
    def closeEvent(self, event):
        """处理窗口关闭事件，清理资源"""
        if self.auto_nav_running:
            self.stop_auto_navigation()
        self.stop_robot()
        event.accept()

    def set_status_label(self, text, status_type="normal"):
        """
        设置状态标签的文本和样式
        
        参数:
            text: 要显示的状态文本
            status_type: 状态类型，可以是 'normal', 'running', 'error', 'manual'
        """
        # 设置模式标签文本
        self.status_label.setText(text)
        
        # 根据状态类型设置不同的样式
        if status_type == "running":
            self.status_label.setStyleSheet("color: #68C190; font-weight: bold;")  # 绿色
        elif status_type == "error":
            self.status_label.setStyleSheet("color: #FF6B6B; font-weight: bold;")  # 红色
        elif status_type == "manual":
            self.status_label.setStyleSheet("color: #4894E3; font-weight: bold;")  # 蓝色
        else:
            self.status_label.setStyleSheet("color: white; font-weight: bold;")    # 白色
        
        # 更新UI
        QApplication.processEvents()

    def simple_exploration_move(self, failure_count):
        """执行简单的探索移动，主要用于没有前沿点时"""
        twist = Twist()
        
        
        # 如果连续失败次数较多，进行旋转
        if failure_count > 1:
            rospy.loginfo("执行360度环境扫描")
            # 旋转一周，寻找新区域
            twist.angular.z = 0.4  # 适中的角速度
            
            start_time = time.time()
            rotation_time = 8.0  # 慢速旋转8秒，接近一个完整的360度
            
            while time.time() - start_time < rotation_time and self.auto_nav_running:
                self.cmd_vel_pub.publish(twist)
                rospy.sleep(0.1)
        else:
            # 简单前进一段距离
            rospy.loginfo("向前探索")
            twist.linear.x = 0.15  # 较慢的线速度
            
            start_time = time.time()
            move_time = 3.0  # 前进3秒
            
            while time.time() - start_time < move_time and self.auto_nav_running:
                self.cmd_vel_pub.publish(twist)
                rospy.sleep(0.1)
        
        # 停止移动
        twist = Twist()
        self.cmd_vel_pub.publish(twist)

    def check_if_making_progress(self):
        """检查导航是否正在取得进展"""
        # 简单实现：检查是否收到速度命令
        try:
            # 记录当前位置
            start_x, start_y = self.get_robot_position()
            
            # 等待一小段时间
            rospy.sleep(1.5)
            
            # 检查是否移动
            current_x, current_y = self.get_robot_position()
            distance_moved = math.sqrt((current_x - start_x)**2 + (current_y - start_y)**2)
            
            return distance_moved > 0.05  # 如果移动了5cm以上，认为有进展
        except:
            return False  # 出现异常时假定没有进展

    def publish_path_to_goal(self, target_x, target_y):
        """发布从当前位置到目标点的简单路径，用于可视化"""
        # 获取当前位置
        robot_x, robot_y = self.get_robot_position()
        
        # 创建路径消息
        path = Path()
        path.header.frame_id = "map"
        path.header.stamp = rospy.Time.now()
        
        # 添加起点
        pose1 = PoseStamped()
        pose1.header = path.header
        pose1.pose.position.x = robot_x
        pose1.pose.position.y = robot_y
        pose1.pose.position.z = 0.0
        pose1.pose.orientation.w = 1.0
        path.poses.append(pose1)
        
        # 添加终点
        pose2 = PoseStamped()
        pose2.header = path.header
        pose2.pose.position.x = target_x
        pose2.pose.position.y = target_y
        pose2.pose.position.z = 0.0
        pose2.pose.orientation.w = 1.0
        path.poses.append(pose2)
        
        # 发布路径
        self.path_pub.publish(path)

    def explore_distant_area(self):
        """探索远距离区域，更积极地探索未知区域"""
        try:
            # 获取当前参数
            params = getattr(self, 'exploration_params', {})
            normal_dist = params.get('normal_exploration_dist', 2.5)
            
            rospy.loginfo(f"启动积极探索模式 (距离: {normal_dist}米)")
            self.append_log("<span style='color:#FFD700;'>启动积极探索</span>")
            
            # 获取当前位置和朝向
            try:
                robot_x, robot_y = self.get_robot_position()
                robot_yaw = self.get_robot_orientation()
            except Exception as e:
                rospy.logerr(f"获取机器人位置失败: {e}")
                return False
            
            # 确定探索方向 - 更积极地寻找未知区域
            best_direction = self.find_best_exploration_direction(aggressive=True)
            
            if best_direction is None:
                # 如果无法找到好的方向，使用随机方向
                best_direction = random.uniform(0, 2*math.pi)
                rospy.loginfo("使用随机方向进行探索")
            
            # 计算目标距离 - 更远的距离，更积极探索
            target_distance = normal_dist
            
            # 计算目标位置
            target_x = robot_x + target_distance * math.cos(best_direction)
            target_y = robot_y + target_distance * math.sin(best_direction)
            
            # 发布目标可视化
            rospy.loginfo(f"积极探索目标: ({target_x:.2f}, {target_y:.2f})")
            try:
                self.publish_path_to_goal(target_x, target_y)
            except:
                rospy.logwarn("无法发布路径可视化")
            
            # 创建导航目标并发送
            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = "map"
            goal.target_pose.header.stamp = rospy.Time.now()
            goal.target_pose.pose.position.x = target_x
            goal.target_pose.pose.position.y = target_y
            goal.target_pose.pose.position.z = 0.0
            
            # 设置目标朝向 - 朝向更远的未知区域
            goal.target_pose.pose.orientation.w = math.cos(best_direction / 2)
            goal.target_pose.pose.orientation.x = 0.0
            goal.target_pose.pose.orientation.y = 0.0
            goal.target_pose.pose.orientation.z = math.sin(best_direction / 2)
            
            if not self.move_base_client:
                rospy.logerr("导航客户端不可用")
                return False
                
            self.move_base_client.send_goal(goal)
            
            # 使用更耐心的等待
            result_timeout = 25.0
            success = False
            
            try:
                success = self.move_base_client.wait_for_result(rospy.Duration(result_timeout))
            except Exception as e:
                rospy.logerr(f"等待导航结果时出错: {e}")
                return False
            
            # 即使部分成功也算成功
            if success and self.move_base_client.get_state() == actionlib.GoalStatus.SUCCEEDED:
                rospy.loginfo("积极探索完全成功")
                return True
            else:
                # 检查移动距离
                try:
                    current_x, current_y = self.get_robot_position()
                    distance_moved = math.sqrt((current_x - robot_x)**2 + (current_y - robot_y)**2)
                    
                    if distance_moved > 0.5:  # 只要前进0.5米就算成功
                        rospy.loginfo(f"积极探索部分完成，移动了{distance_moved:.2f}米")
                        return True
                    else:
                        rospy.logwarn("积极探索几乎没有进展")
                        return False
                except:
                    rospy.logerr("计算移动距离失败")
                    return False
        except Exception as e:
            rospy.logerr(f"积极探索功能发生错误: {e}")
            return False

    def find_best_exploration_direction(self, aggressive=False, very_aggressive=False):
        """寻找最佳的探索方向 - 支持标准、积极和非常积极三种模式"""
        try:
            # 获取当前参数
            params = getattr(self, 'exploration_params', {})
            cluster_weight = params.get('cluster_weight', 5.0)
            obstacle_weight = params.get('obstacle_weight', 1.5)
            
            if self.map_data is None:
                return None
            
            # 获取机器人位置并转换为地图坐标
            robot_x, robot_y = self.get_robot_position()
            map_data = self.map_data
            resolution = map_data.info.resolution
            
            robot_mx = int((robot_x - map_data.info.origin.position.x) / resolution)
            robot_my = int((robot_y - map_data.info.origin.position.y) / resolution)
            
            # 检查坐标是否有效
            width = map_data.info.width
            height = map_data.info.height
            if robot_mx < 0 or robot_mx >= width or robot_my < 0 or robot_my >= height:
                return None
            
            # 将地图数据转换为numpy数组
            data = np.array(map_data.data, dtype=np.int8).reshape(height, width)
            
            # 定义探测角度和距离参数 - 根据模式调整
            if very_aggressive:
                angles = np.linspace(0, 2*math.pi, 36, endpoint=False)  # 更多方向
                max_dist = min(100, min(width, height) // 2)  # 非常远的探测距离
                start_dist = 25  # 从更远的地方开始考虑未知区域
            elif aggressive:
                angles = np.linspace(0, 2*math.pi, 32, endpoint=False)
                max_dist = min(80, min(width, height) // 2)  
                start_dist = 15
            else:
                angles = np.linspace(0, 2*math.pi, 24, endpoint=False)
                max_dist = min(50, min(width, height) // 2)
                start_dist = 10
            
            # 每个方向的评分
            direction_scores = []
            
            # 检查每个方向
            for angle in angles:
                unknown_count = 0
                free_count = 0
                obstacle_count = 0
                weighted_unknown = 0  # 加权未知区域计数
                unknown_clusters = 0  # 未知区域簇
                last_was_unknown = False
                
                # 沿该方向前进并检查单元格
                for dist in range(start_dist, max_dist):
                    # 计算探测点
                    px = int(robot_mx + dist * math.cos(angle))
                    py = int(robot_my + dist * math.sin(angle))
                    
                    # 确保不会越界
                    if px < 0 or px >= width or py < 0 or py >= height:
                        break
                    
                    # 检查单元格状态
                    cell_value = data[py, px]
                    
                    if cell_value == -1:  # 未知区域
                        unknown_count += 1
                        
                        # 检测未知区域簇
                        if not last_was_unknown:
                            unknown_clusters += 1
                        last_was_unknown = True
                        
                        # 根据模式计算加权值
                        if very_aggressive:
                            # 非常积极模式下，远处未知区域权重更大
                            distance_weight = min(3.0, dist / 25.0)
                            weighted_unknown += distance_weight
                        elif aggressive:
                            distance_weight = min(2.0, dist / 30.0)
                            weighted_unknown += distance_weight
                        else:
                            weighted_unknown += 1.0
                    else:
                        last_was_unknown = False
                        
                        if cell_value >= 0 and cell_value < 20:  # 空闲区域
                            free_count += 1
                        elif cell_value > 65:  # 障碍物
                            obstacle_count += 1
                            # 如果障碍物太近且处于积极模式，提前结束
                            if dist < 15 and (aggressive or very_aggressive):
                                break
                
                # 计算方向得分 - 根据模式使用不同的评分策略
                if very_aggressive:
                    # 非常积极模式：极度重视未知区域，特别是远处的大片未知区域
                    score = (weighted_unknown * 3.0 + unknown_count * 0.2 + unknown_clusters * cluster_weight) - (obstacle_count * (obstacle_weight * 0.2))
                elif aggressive:
                    # 积极模式：重视未知区域和簇
                    score = weighted_unknown * 2.0 + unknown_count * 0.5 + unknown_clusters * (cluster_weight * 0.4) - obstacle_count * (obstacle_weight * 0.7)
                else:
                    # 标准模式：平衡考虑
                    score = unknown_count + unknown_clusters * (cluster_weight * 0.2) - obstacle_count * (obstacle_weight * 0.3)
                
                direction_scores.append(score)
            
            # 找出得分最高的方向
            if len(direction_scores) > 0 and max(direction_scores) > 0:
                best_index = np.argmax(direction_scores)
                return angles[best_index]
            
            # 如果所有方向得分都不好，随机选择
            return random.uniform(0, 2*math.pi)
        except Exception as e:
            rospy.logerr(f"寻找探索方向出错: {e}")
            return None

    def detect_spinning_behavior(self):
        """检测机器人是否在原地打转或小范围徘徊"""
        # 获取当前参数
        params = getattr(self, 'exploration_params', {})
        recent_goals_num = params.get('recent_goals_num', 5)
        spin_threshold = params.get('spin_threshold', 1.0)
        near_goal_threshold = params.get('near_goal_threshold', 1.8)
        
        # 需要至少记录了几个目标点才能判断
        recent_goals = self.exploration_history.get_recent_goals(recent_goals_num)
        if len(recent_goals) < 3:
            return False
        
        # 计算最近几个目标点的中心
        center_x = sum(x for x, y in recent_goals) / len(recent_goals)
        center_y = sum(y for x, y in recent_goals) / len(recent_goals)
        
        # 计算这些点到中心的平均距离
        avg_dist = sum(math.sqrt((x - center_x)**2 + (y - center_y)**2) for x, y in recent_goals) / len(recent_goals)
        
        # 获取当前位置
        robot_x, robot_y = self.get_robot_position()
        
        # 如果平均距离小于阈值，说明机器人可能在原地打转
        rospy.loginfo(f"最近目标平均距离: {avg_dist:.2f}米 (阈值: {spin_threshold}米)")
        if avg_dist < spin_threshold:
            return True
        
        # 检查最近目标是否都在机器人附近
        near_count = 0
        for goal_x, goal_y in recent_goals[-3:]:  # 只检查最近3个
            dist_to_robot = math.sqrt((goal_x - robot_x)**2 + (goal_y - robot_y)**2)
            if dist_to_robot < near_goal_threshold:
                near_count += 1
        
        if near_count >= 3:  # 如果最近3个目标都很近
            rospy.logwarn(f"检测到连续近距离目标: {near_count}/3")
            return True
        
        return False

    def force_global_exploration(self):
        """强制进行全局探索，直接朝向大片未知区域"""
        # 获取当前参数
        params = getattr(self, 'exploration_params', {})
        global_dist = params.get('global_exploration_dist', 3.5)
        
        rospy.loginfo(f"检测到机器人在原地打转，启动全局探索 (距离: {global_dist}米)")
        self.append_log("<span style='color:#FF6B6B;'>检测到打转行为，启动全局探索</span>")
        
        # 获取当前位置
        robot_x, robot_y = self.get_robot_position()
        
        # 使用更激进的参数寻找探索方向
        best_direction = self.find_best_exploration_direction(very_aggressive=True)
        
        if best_direction is None:
            # 如果找不到好的方向，随机选择一个方向
            best_direction = random.uniform(0, 2*math.pi)
        
        # 计算目标位置
        target_x = robot_x + global_dist * math.cos(best_direction)
        target_y = robot_y + global_dist * math.sin(best_direction)
        
        # 发布目标可视化
        rospy.loginfo(f"全局探索目标: ({target_x:.2f}, {target_y:.2f})")
        self.publish_path_to_goal(target_x, target_y)
        
        # 创建导航目标
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = target_x
        goal.target_pose.pose.position.y = target_y
        goal.target_pose.pose.position.z = 0.0
        
        # 设置目标朝向
        goal.target_pose.pose.orientation.w = math.cos(best_direction / 2)
        goal.target_pose.pose.orientation.x = 0.0
        goal.target_pose.pose.orientation.y = 0.0
        goal.target_pose.pose.orientation.z = math.sin(best_direction / 2)
        
        # 发送导航目标
        if not self.move_base_client:
            rospy.logerr("导航客户端不可用")
            return False
            
        self.move_base_client.send_goal(goal)
        
        # 等待较长时间，确保机器人有足够时间前进
        try:
            success = self.move_base_client.wait_for_result(rospy.Duration(35.0))
            
            if success and self.move_base_client.get_state() == actionlib.GoalStatus.SUCCEEDED:
                rospy.loginfo("全局探索完全成功")
                return True
            else:
                # 即使没有完全到达目标，也检查是否有所前进
                current_x, current_y = self.get_robot_position()
                distance_moved = math.sqrt((current_x - robot_x)**2 + (current_y - robot_y)**2)
                
                if distance_moved > 1.0:  # 至少前进了1米
                    rospy.loginfo(f"全局探索部分完成，移动了{distance_moved:.2f}米")
                    return True
                else:
                    rospy.logwarn("全局探索失败")
                    return False
        except Exception as e:
            rospy.logerr(f"全局探索等待结果出错: {e}")
            return False

    # 在TeleopWithExplorationGUI类中添加一个新方法来创建参数调试界面
    def create_exploration_params_widget(self):
        """创建探索参数调整面板"""
        params_widget = QWidget()
        layout = QVBoxLayout(params_widget)
        layout.setContentsMargins(10, 10, 10, 10)
        layout.setSpacing(8)
        
        # 标题
        title_label = QLabel("探索参数调整")
        title_label.setAlignment(Qt.AlignCenter)
        title_label.setStyleSheet("font-size: 16px; font-weight: bold; margin-bottom: 10px;")
        layout.addWidget(title_label)
        
        # ---------- 打转检测参数 ----------
        spinning_group = QGroupBox("打转检测参数")
        spinning_layout = QGridLayout()
        
        # 历史目标点数量
        spinning_layout.addWidget(QLabel("历史目标点数:"), 0, 0)
        self.recent_goals_slider = QSlider(Qt.Horizontal)
        self.recent_goals_slider.setMinimum(3)
        self.recent_goals_slider.setMaximum(10)
        self.recent_goals_slider.setValue(5)
        self.recent_goals_slider.setTickPosition(QSlider.TicksBelow)
        self.recent_goals_slider.setTickInterval(1)
        self.recent_goals_value = QLabel("5")
        spinning_layout.addWidget(self.recent_goals_slider, 0, 1)
        spinning_layout.addWidget(self.recent_goals_value, 0, 2)
        self.recent_goals_slider.valueChanged.connect(
            lambda v: self.recent_goals_value.setText(str(v)))
        
        # 打转距离阈值
        spinning_layout.addWidget(QLabel("打转距离阈值(m):"), 1, 0)
        self.spin_threshold_slider = QSlider(Qt.Horizontal)
        self.spin_threshold_slider.setMinimum(5)
        self.spin_threshold_slider.setMaximum(20)
        self.spin_threshold_slider.setValue(10)  # 1.0米，显示时除以10
        self.spin_threshold_slider.setTickPosition(QSlider.TicksBelow)
        self.spin_threshold_value = QLabel("1.0")
        spinning_layout.addWidget(self.spin_threshold_slider, 1, 1)
        spinning_layout.addWidget(self.spin_threshold_value, 1, 2)
        self.spin_threshold_slider.valueChanged.connect(
            lambda v: self.spin_threshold_value.setText(f"{v/10:.1f}"))
        
        # 目标近距离阈值
        spinning_layout.addWidget(QLabel("目标近距阈值(m):"), 2, 0)
        self.near_goal_slider = QSlider(Qt.Horizontal)
        self.near_goal_slider.setMinimum(10)
        self.near_goal_slider.setMaximum(30)
        self.near_goal_slider.setValue(18)  # 1.8米，显示时除以10
        self.near_goal_slider.setTickPosition(QSlider.TicksBelow)
        self.near_goal_value = QLabel("1.8")
        spinning_layout.addWidget(self.near_goal_slider, 2, 1)
        spinning_layout.addWidget(self.near_goal_value, 2, 2)
        self.near_goal_slider.valueChanged.connect(
            lambda v: self.near_goal_value.setText(f"{v/10:.1f}"))
        
        # 连续打转检测阈值
        spinning_layout.addWidget(QLabel("连续打转阈值:"), 3, 0)
        self.spin_count_slider = QSlider(Qt.Horizontal)
        self.spin_count_slider.setMinimum(1)
        self.spin_count_slider.setMaximum(5)
        self.spin_count_slider.setValue(3)
        self.spin_count_slider.setTickPosition(QSlider.TicksBelow)
        self.spin_count_slider.setTickInterval(1)
        self.spin_count_value = QLabel("3")
        spinning_layout.addWidget(self.spin_count_slider, 3, 1)
        spinning_layout.addWidget(self.spin_count_value, 3, 2)
        self.spin_count_slider.valueChanged.connect(
            lambda v: self.spin_count_value.setText(str(v)))
        
        spinning_group.setLayout(spinning_layout)
        layout.addWidget(spinning_group)
        
        # ---------- 探索距离参数 ----------
        distance_group = QGroupBox("探索距离参数")
        distance_layout = QGridLayout()
        
        # 普通探索距离
        distance_layout.addWidget(QLabel("普通探索距离(m):"), 0, 0)
        self.normal_dist_slider = QSlider(Qt.Horizontal)
        self.normal_dist_slider.setMinimum(10)
        self.normal_dist_slider.setMaximum(30)
        self.normal_dist_slider.setValue(25)  # 2.5米，显示时除以10
        self.normal_dist_slider.setTickPosition(QSlider.TicksBelow)
        self.normal_dist_value = QLabel("2.5")
        distance_layout.addWidget(self.normal_dist_slider, 0, 1)
        distance_layout.addWidget(self.normal_dist_value, 0, 2)
        self.normal_dist_slider.valueChanged.connect(
            lambda v: self.normal_dist_value.setText(f"{v/10:.1f}"))
        
        # 全局探索距离
        distance_layout.addWidget(QLabel("全局探索距离(m):"), 1, 0)
        self.global_dist_slider = QSlider(Qt.Horizontal)
        self.global_dist_slider.setMinimum(20)
        self.global_dist_slider.setMaximum(50)
        self.global_dist_slider.setValue(35)  # 3.5米，显示时除以10
        self.global_dist_slider.setTickPosition(QSlider.TicksBelow)
        self.global_dist_value = QLabel("3.5")
        distance_layout.addWidget(self.global_dist_slider, 1, 1)
        distance_layout.addWidget(self.global_dist_value, 1, 2)
        self.global_dist_slider.valueChanged.connect(
            lambda v: self.global_dist_value.setText(f"{v/10:.1f}"))
        
        distance_group.setLayout(distance_layout)
        layout.addWidget(distance_group)
        
        # ---------- 其他探索参数 ----------
        other_group = QGroupBox("其他探索参数")
        other_layout = QGridLayout()
        
        # 未知区域聚类权重
        other_layout.addWidget(QLabel("未知区域簇权重:"), 0, 0)
        self.cluster_weight_slider = QSlider(Qt.Horizontal)
        self.cluster_weight_slider.setMinimum(1)
        self.cluster_weight_slider.setMaximum(10)
        self.cluster_weight_slider.setValue(5)
        self.cluster_weight_slider.setTickPosition(QSlider.TicksBelow)
        self.cluster_weight_value = QLabel("5.0")
        other_layout.addWidget(self.cluster_weight_slider, 0, 1)
        other_layout.addWidget(self.cluster_weight_value, 0, 2)
        self.cluster_weight_slider.valueChanged.connect(
            lambda v: self.cluster_weight_value.setText(f"{v:.1f}"))
        
        # 障碍物权重倍数
        other_layout.addWidget(QLabel("障碍物惩罚系数:"), 1, 0)
        self.obstacle_weight_slider = QSlider(Qt.Horizontal)
        self.obstacle_weight_slider.setMinimum(1)
        self.obstacle_weight_slider.setMaximum(20)
        self.obstacle_weight_slider.setValue(15)  # 1.5，显示时除以10
        self.obstacle_weight_slider.setTickPosition(QSlider.TicksBelow)
        self.obstacle_weight_value = QLabel("1.5")
        other_layout.addWidget(self.obstacle_weight_slider, 1, 1)
        other_layout.addWidget(self.obstacle_weight_value, 1, 2)
        self.obstacle_weight_slider.valueChanged.connect(
            lambda v: self.obstacle_weight_value.setText(f"{v/10:.1f}"))
        
        other_group.setLayout(other_layout)
        layout.addWidget(other_group)
        
        # 应用按钮
        apply_button = AnimatedButton("应用参数")
        apply_button.clicked.connect(self.apply_exploration_params)
        layout.addWidget(apply_button)
        
        # 填充空白
        layout.addStretch(1)
        
        return params_widget

    def apply_exploration_params(self):
        """应用探索参数更改"""
        self.exploration_params = {
            # 打转检测参数
            'recent_goals_num': self.recent_goals_slider.value(),
            'spin_threshold': self.spin_threshold_slider.value() / 10.0,
            'near_goal_threshold': self.near_goal_slider.value() / 10.0,
            'spin_count_threshold': self.spin_count_slider.value(),
            
            # 探索距离参数
            'normal_exploration_dist': self.normal_dist_slider.value() / 10.0,
            'global_exploration_dist': self.global_dist_slider.value() / 10.0,
            
            # 其他参数
            'cluster_weight': self.cluster_weight_slider.value(),
            'obstacle_weight': self.obstacle_weight_slider.value() / 10.0,
        }
        
        self.append_log("<span style='color:#68C190;'>已更新探索参数</span>")
        rospy.loginfo(f"已更新探索参数: {self.exploration_params}")

    def show_exploration_params(self):
        """显示探索参数设置窗口"""
        # 创建一个新窗口作为参数控制面板
        self.params_window = QMainWindow()
        self.params_window.setWindowTitle("探索参数设置")
        self.params_window.setGeometry(100, 100, 400, 550)
        
        # 创建参数控制面板
        self.params_widget = self.create_exploration_params_widget()
        self.params_window.setCentralWidget(self.params_widget)
        
        # 显示窗口
        self.params_window.show()

# 主函数
def main():
    app = QApplication(sys.argv)
    window = TeleopWithExplorationGUI()
    window.show()
    sys.exit(app.exec_())

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass